#include "map_conversion/utility.hpp"
#include "map_conversion/tic_toc.h"




int main(int argc, char** argv)
{
   google::InitGoogleLogging(argv[0]);
   FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
   FLAGS_alsologtostderr = 1;

   ros::init(argc, argv, "global_local_costmap_node");
   ros::NodeHandle nh; //创建ros句柄
   //创建读取参数的对象
   YAML::Node config = YAML::LoadFile(WORK_SPACE_PATH+"/config/params.yaml"); 

  //创建点云处理对象，传入参数为前面创建的读取参数的对象
   Pointcloud2dProcess *pointcloud_2d_process_ptr = new Pointcloud2dProcess(config);
   //Pointcloud3dProcess *pointcloud_3d_process_ptr = new Pointcloud3dProcess(config);

   //创建ros发布和接受话题对象
   //std::shared_ptr<CloudPublisher> global_cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "global_cloud", "/camera_init", 100);
   //std::shared_ptr<CloudPublisher> global_3d_grid_pub_ptr = std::make_shared<CloudPublisher>(nh, "global_inflation_map", "/camera_init", 100);
   ros::Publisher global_local_costmap_pub = nh.advertise<nav_msgs::OccupancyGrid>("/global_local_map", 100);

   nav_msgs::OccupancyGrid global_grid_map_msg;
   TicToc t_map; //创建计时对象

   //导入全局地图并初始化，其中转换为2d栅格格式
   pointcloud_2d_process_ptr->global_map_init();
   pointcloud_2d_process_ptr->Pointcloud_to_2d_grid(pointcloud_2d_process_ptr->global_map_after_filter,global_grid_map_msg,"global_map");
   global_grid_map_msg.header.frame_id = "global_map";
   
   //局部地图接受
   std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, pointcloud_2d_process_ptr->local_cloud_topic, 100000);
   
   nav_msgs::OccupancyGrid local_grid_map_msg; // local point的实时local costmap
   std::deque<CloudData> cloud_data_buff; //实时点云buffer

   //pointcloud_3d_process_ptr->pointcloud_to_3dGridMap(pointcloud_2d_process_ptr->global_map_after_filter,grid_cloud_data);
   printf("global map init time %f ms \n", t_map.toc()); //打印初始化需要的时间

   ros::Rate loop_rate(10.0); 
    while(ros::ok())
  {
     ros::spinOnce();
     cloud_sub_ptr->ParseData(cloud_data_buff);//从封装好的点云话题接受类内取数据
     if(cloud_data_buff.size() > 0)
     {

        CloudData cloud_data = cloud_data_buff.front();  //存到buffer

        pointcloud_2d_process_ptr->Pointcloud_to_2d_grid(cloud_data,local_grid_map_msg,"local msg in global msg process");


        local_grid_map_msg.header.frame_id = "map";
        global_local_costmap_pub.publish(local_grid_map_msg);

        cloud_data_buff.pop_front();

     }

     loop_rate.sleep();
  }
   return 0;
}
